/*
 * Copyright Nucleron R&D LLC 2016
 *
 * This file is licensed under the terms of YAPL,
 * see License.txt for details.
 */

// #include <libopencm3/stm32/rcc.h>
// #include <libopencm3/cm3/nvic.h>
// #include <libopencm3/cm3/systick.h>
#include<Arduino.h>
#include <plc_config.h>
#include <plc_abi.h>
#include <plc_dbg.h>
#include <plc_diag.h>
#include <plc_tick.h>
#include <frac_div.h>
#include <plc_hw.h>
#include <plc_iom.h>
uint32_t _period,_tickCount;
static bool systick_set_period(uint32_t period)
{
   _period=period;
   _tickCount=0;
   return 1;
}

#ifndef PLC_TICK_MIN_PER
#define PLC_TICK_MIN_PER    100000ULL
#endif//PLC_TICK_MIN_PER
#define TICK_THR_PER 500000000ULL
static frac_div_t systick_ctrl;

#define TICK_STATE_HIGH 0
#define TICK_STATE_MID  1
#define TICK_STATE_LOW  2
static uint32_t tick_state = TICK_STATE_HIGH;

uint64_t plc_tick_time = PLC_TICK_MIN_PER;

volatile bool plc_tick_flag = false;

extern "C" void plc_irq_stub(void);
//// We need some RTOS for such behaviour
static bool dl_post_flag = true;
static bool dl_fail_flag = false;
//// We need some RTOS for such behaviour

void plc_tick_init(void)
{
}

void plc_tick_poll(void)
{

   if (dl_fail_flag)
   {
       plc_diag_status |= PLC_DIAG_ERR_DEADLINE;

       if (dl_post_flag)
       {
           dl_post_flag = false;
           plc_app_stop();/* Must stop the app now! */
       }
   }

}

void sys_tick_handler(void)
{
    if(millis()<_tickCount)return;
    _tickCount=millis()+_period;
    //Soft watchdog
    if (plc_tick_flag)
    {

       if (plc_dbg_mode)
       {
           //In debug mode we stop the program
           dl_fail_flag = true;
       }
       else
       {
           //In normal mode we reset PLC
           plc_irq_stub();
       }

        plc_irq_stub();
    }

    switch (tick_state)
    {
    case TICK_STATE_MID:
    case TICK_STATE_HIGH:
    default:
        // systick_set_reload(PLC_RCC_AHB_FREQ * (uint32_t)frac_div_icalc(&systick_ctrl) - 1);
        plc_tick_flag = true;
        break;

    // case TICK_STATE_LOW:
    //     plc_tick_flag = frac_div_run(&systick_ctrl);
    //     break;
    }
}

//Tick period in ns
void plc_tick_setup(uint64_t tick_next, uint64_t tick_period)
{
    (void)tick_next;//disable warning

    if (PLC_TICK_MIN_PER > tick_period)
    {
        tick_period = PLC_TICK_MIN_PER;
    }

    plc_tick_time = tick_period;

    if (TICK_THR_PER > tick_period)
    {
        //Переменная частота работы системного таймера.
        if (systick_set_period((uint32_t)(tick_period/1000000ULL)))
        {
            //Примерно до 0.1с
            tick_state = TICK_STATE_HIGH;
            frac_div_init(&systick_ctrl, tick_period, 1000000ULL); //Коррекция периода
        }
        else
        {
            //До 0.5с
            tick_state = TICK_STATE_MID;
            systick_set_period((uint32_t)(tick_period/8000ULL));
            frac_div_init(&systick_ctrl, tick_period, 8000ULL); //Коррекция периода
        }
    }
    else
    {
        // //Постоянная частота работы системного таймера.
        tick_state = TICK_STATE_LOW;
        systick_set_period(500);//500мкс
        frac_div_init(&systick_ctrl, tick_period, 500000ULL);
    }

    // systick_counter_enable();
    // systick_interrupt_enable();
}
